/*
 * File:   mcVector.h
 *
 * @author Thales Luis Rodrigues Sabino
 * @date   2009
 */

#ifndef _MCVECTOR_H_
#define _MCVECTOR_H_

#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <iostream>
#include <string>

namespace mc {

    template <class T> class vec2;
    template <class T> class vec3;
    template <class T> class vec4;

    template <class T> class matrix2;
    template <class T> class matrix3;
    template <class T> class matrix4;

    typedef enum { MC_X_AXIS, MC_Y_AXIS, MC_Z_AXIS } cartesian_axes;

    /**
     * @brief Classe template para vetores 2D.
     */
    template<class T>
    class vec2 {

        /// Atributos
        public:

            typedef T value_type;

            union {
                struct {
                    T x;
                    T y;
                };

                T _array[2];     // array access
            };

        /// Métodos
        public:            

            /**
             * @brief Construtor padrão. Constroi um vetor com o valor t
             *
             * @param t - Valor para ser setado em todas as componentes
             */
            vec2( const T &t = T() ) {
                this->x = t;
                this->y = t;
            }
            

            /**
            * @brief Constrói a partir de um array
            *
            * @param a - Array para construir o vetor
            */
            vec2( const T *a ) {
                this->x = a[0];
                this->y = a[1];
            } 
           

            /**
             * @brief Constrói um vetor explicitamente
             *
             * @param x - Compomente X
             * @param y - Componente Y
             */
            vec2( const T x, const T y ) {                
                this->x = x;
                this->y = y;
            }
    

            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec2( vec2<T> &v ) {
                this->x = v.x;
                this->y = v.y;
            }
   

            vec2( vec3<T> &v ) {
                this->x = v.x;
                this->y = v.y;
            }
        
            
            vec2( vec4<T> &v ) {
                this->x = v.x;
                this->y = v.y;
            }


            /**
             * @brief Constrói um vetor que representa um eixo cartesiano
             *
             * @param axis - Deve ser um de cartesian_axes
             */
            vec2( cartesian_axes axis ) {
                this->set_value(axis);
            }

            /**
             * @brief Seta os valores do vetor explicitamente
             *
             * @param x
             * @param y
             */
            void set_value( const T x, const T y ) {
                this->x = x;
                this->y = y;
            }

            
            /**
             * @brief Seta o valor correspondente a um dos eixos cartesianos
             * 
             * @param axis - Deve ser um valor do tipo cartesian_axes
             */
            void set_value( cartesian_axes axis ) {
                switch( axis ) {

                    case MC_X_AXIS:
                        this->set_value(1,0);
                        break;

                    case MC_Y_AXIS:
                        this->set_value(0,1);
                        break;

                    default:
                        break;
                        
                }
            }
                    

            /**
             * @brief Retorna o tamanho desse vetor
             */
            inline int size() const { return 2; }

            /**
             * @brief Operador [] para acesso ao vetor
             *
             * @param i - Índice de acesso
             */
            T& operator [] ( int i ) {
                assert( i >= 0 && i < this->size() );
                return this->_array[i];
            }


            /**
             * @brief Operador soma de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec2<T>& operator + ( const vec2<T> &v ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x + v.x;
                vr->y = this->y + v.y;
                return *vr;
            }


            /**
             * @brief Operador subtração de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec2<T>& operator - ( const vec2<T> &v ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x - v.x;
                vr->y = this->y - v.y;
                return *vr;
            }



            /**
             * @brief Operador multiplicação de componentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec2<T>& operator * ( const vec2<T> &v ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x * v.x;
                vr->y = this->y * v.y;
                return *vr;
            }


            /**
             * @brief Operador divisão de compomentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec2<T>& operator / ( const vec2<T> &v ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x / v.x;
                vr->y = this->y / v.y;
                return *vr;
            }



            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec2<T>& operator * ( const T s ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x * s;
                vr->y = this->y * s;
                return *vr;
            }


            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec2<T>& operator / ( const T s ) {
                vec2<T> *vr = new vec2<T>();
                vr->x = this->x / s;
                vr->y = this->y / s;
                return *vr;
            }


            /**
             * @brief Acumulador +=
             *
             * @param v
             */
            vec2<T>& operator += ( const vec2<T> &v ) {
                this->x += v.x;
                this->y += v.y;
                return *this;
            }


            /**
             * @brief Acumulador -=
             *
             * @param v
             */
            vec2<T>& operator -= ( const vec2<T> &v ) {
                this->x -= v.x;
                this->y -= v.y;
                return *this;
            }


            /**
             * @brief Acumulador *=
             *
             * @param v
             */
            vec2<T>& operator *= ( const vec2<T> &v ) {
                this->x *= v.x;
                this->y *= v.y;
                return *this;
            }
 

            /**
             * @brief Acumulador /=
             *
             * @param v
             */
            vec2<T>& operator /= ( const vec2<T> &v ) {
                this->x /= v.x;
                this->y /= v.y;
                return *this;
            }

            /**
             * @brief Operador de atribuição
             *
             * @param v
             */
            vec2<T>& operator = ( const vec2<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                return *this;
            }
            

            /**
             * @brief Operador para multiplicação de uma matriz por um vetor
             *
             * @param m - Matriz
             * @param v - Vetor
             */
            friend vec2<T>& operator * ( const matrix2<T> m, const vec2<T> &v ) {
                vec2<T> *vr = new vec2<T>();
                vr.x = v.x * m._00 + v.y * m._01;
                vr.y = v.x * m._10 + v.y * m._11;
                return *vr;
            }

            /**
             * @brief Comparador de igualdade
             *
             * @param v
             */
            friend bool operator == ( const vec2<T> &v1, const vec2<T> &v2 ) {
                return ( v1.x == v2.x && v1.y == v2.y ) ? true : false;
            }

            /**
             * @brief Comparador de desigualdade
             *
             * @param v
             */
            friend bool operator != ( const vec2<T> &v1, const vec2<T> &v2 ) {
                return ( v1.x != v2.x || v1.y != v2.y ) ? true : false;
            }
            

            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const vec2<T> &v ) {
                char *pOut = new char[256];
                sprintf( pOut, "(%7.4f, %7.4f)", v.x, v.y );
                out << pOut;
                delete [] pOut;
                return out;
            }


    };


    /**
     * @brief Classe template para vetores 3D.
     */
    template <class T>
    class vec3 {
        /// Atributos
        public:

            typedef T value_type;

            union {
                struct {
                    T x;
                    T y;
                    T z;
                };

                T _array[3];
            };

        /// Métodos
        public:

            /**
             * @brief Construtor padrão. Constroi um vetor com o valor t
             *
             * @param t - Valor para ser setado em todas as componentes
             */
            vec3( const T &t = T() ) {
                this->x = t;
                this->y = t;
                this->z = t;
            }


            /**
            * @brief Constrói a partir de um array
            *
            * @param a - Array para construir o vetor
            */
            vec3( const T *a ) {
                this->x = a[0];
                this->y = a[1];
                this->z = a[2];
            }


            /**
             * @brief Constrói um vetor explicitamente
             *
             * @param x - Compomente X
             * @param y - Componente Y
             */
            vec3( const T x, const T y, const T z ) {
                this->x = x;
                this->y = y;
                this->z = z;
            }
     

            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec3( vec2<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = (T) 0;
            }


            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec3( vec3<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
            }
            

            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec3( vec4<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
            }


            /**
             * @brief Constrói um vetor que representa um eixo cartesiano
             *
             * @param axis - Deve ser um de cartesian_axes
             */
            vec3( cartesian_axes axis ) {
                this->set_value(axis);
            }

            /**
             * @brief Seta os valores do vetor explicitamente
             *
             * @param x
             * @param y
             * @param z
             */
            void set_value( const T x, const T y, const T z ) {
                this->x = x;
                this->y = y;
                this->z = z;
            }


            /**
             * @brief Seta o valor correspondente a um dos eixos cartesianos
             *
             * @param axis - Deve ser um valor do tipo cartesian_axes
             */
            void set_value( cartesian_axes axis ) {
                switch( axis ) {

                    case MC_X_AXIS:
                        this->set_value( (T)1, (T)0, (T)0 );
                        break;

                    case MC_Y_AXIS:
                        this->set_value( (T)0, (T)1, (T)0 );
                        break;

                    case MC_Z_AXIS:
                        this->set_value( (T)0, (T)0, (T)1 );

                    default:
                        break;

                }
            }


            /**
             * @brief Retorna o tamanho desse vetor
             */
            inline int size() const { return 3; }
            

            /**
             * @brief Operador [] para acesso ao vetor
             *
             * @param i - Índice de acesso
             */
            T& operator [] ( int i ) {
                assert( i >= 0 && i < this->size() );
                return _array[i];
            }

            /**
             * @brief Operador soma de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec3<T>& operator + ( const vec3<T> &v ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = this->x + v.x;
                vr->y = this->y + v.y;
                vr->z = this->z + v.z;
                return *vr;
            }

            /**
             * @brief Operador subtração de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec3<T>& operator - ( const vec3<T> &v ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = this->x - v.x;
                vr->y = this->y - v.y;
                vr->z = this->z - v.z;
                return *vr;
            }


            /**
             * @brief Operador multiplicação das componentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec3<T>& operator * ( const vec3<T> &v ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = this->x * v.x;
                vr->y = this->y * v.y;
                vr->z = this->z * v.z;
                return *vr;
            }


            /**
             * @brief Operador divisão de compomentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec3<T>& operator / ( const vec3<T> &v ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = this->x / v.x;
                vr->y = this->y / v.y;
                vr->z = this->z / v.z;
                return *vr;
            }


            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec3<T>& operator * ( const T s ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = this->x * s;
                vr->y = this->y * s;
                vr->z = this->z * s;
                return *vr;
            }


            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec3<T>& operator / ( const T s ) {
                vec3<T> *vr = new vec3<T>();
                vr.x = this->x / s;
                vr.y = this->y / s;
                vr.z = this->z / s;
                return *vr;
            }



            /**
             * @brief Acumulador +=
             *
             * @param v
             */
            vec3<T>& operator += ( const vec3<T> &v ) {
                this->x += v.x;
                this->y += v.y;
                this->z += v.z;
                return *this;
            }


            /**
             * @brief Acumulador -=
             *
             * @param v
             */
            vec3<T>& operator -= ( const vec3<T> &v ) {
                this->x -= v.x;
                this->y -= v.y;
                this->z -= v.z;
                return *this;
            }


            /**
             * @brief Acumulador *=
             *
             * @param v
             */
            vec3<T>& operator *= ( const vec3<T> &v ) {
                this->x *= v.x;
                this->y *= v.y;
                this->z *= v.z;
                return *this;
            }


            /**
             * @brief Acumulador /=
             *
             * @param v
             */
            vec3<T>& operator /= ( const vec3<T> &v ) {
                this->x /= v.x;
                this->y /= v.y;
                this->z /= v.z;
                return *this;
            }


            /**
             * @brief Cross-product
             *
             * Computa o cross-product entre dois vetores e armazena neste
             *
             * @param v1
             * @param v2
             */
            void cross( const vec3<T> &v1, const vec3<T> &v2 ) {
                vec3<T> c;
                c.x = v1.y * v2.z - v1.z * v2.y;
                c.y = v1.z * v2.x - v1.x * v2.z;
                c.z = v1.x * v2.y - v1.y * v2.x;

                this->x = c.x;
                this->y = c.y;
                this->z = c.z;
            }


            /**
             * @brief Cross-product
             *
             * Computa o cross-product entre um vetor e este
             *
             * @param v
             */
            void cross( const vec3<T> &v ) {
                vec3<T> c;
                c.x = this->y * v.z - this->z * v.y;
                c.y = this->z * v.x - this->x * v.z;
                c.z = this->x * v.y - this->y * v.x;

                this->x = c.x;
                this->y = c.y;
                this->z = c.z;
            }


            /**
             * @brief Operador de atribuição
             *
             * @param v
             */
            vec3<T>& operator = ( const vec3<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
                return *this;
            }


            /**
             * @brief Operador para aplicar uma matriz 3x3 neste vetor
             *
             * @param v
             */
            friend vec3<T>& operator * ( const matrix3<T> &m, const vec3<T> &v ) {
                vec3<T> *vr = new vec3<T>();
                vr->x = v.x * m._00 + v.y * m._01 + v.z * m._02;
                vr->y = v.x * m._10 + v.y * m._11 + v.z * m._12;
                vr->z = v.x * m._20 + v.y * m._21 + v.z * m._22;
                return *vr;
            }


            /**
             * @brief Comparador de igualdade
             *
             * @param v
             */
            friend bool operator == ( const vec3<T> &v1, const vec3<T> &v2 ) {
                return ( v1.x == v2.x && v1.y == v2.y && v1.z == v2.z ) ? true : false;
            }

            /**
             * @brief Comparador de desigualdade
             *
             * @param v
             */
            friend bool operator != ( const vec3<T> &v1, const vec3<T> &v2 ) {
                return ( v1.x != v2.x || v1.y != v2.y || v1.z != v2.z ) ? true : false;
            }
            

            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const vec3<T> &v ) {
                char *pOut = new char[256];
                sprintf( pOut, "(%7.4f, %7.4f, %7.4f)", v.x, v.y, v.z );
                out << pOut;
                delete [] pOut;
                return out;
            }



    };


    /**
     * @brief Classe template para vetores 4D.
     */
    template <class T>
    class vec4 {
        /// Atributos
        public:

            typedef T type_value;

            union {
                struct {
                    T x;
                    T y;
                    T z;
                    T w;
                };

                T _array[4];
            };

        /// Métodos
        public:

            /**
             * @brief Construtor padrão. Constroi um vetor com o valor t
             *
             * @param t - Valor para ser setado em todas as componentes
             */
            vec4( const T &t = T() ) {
                this->x = t;
                this->y = t;
                this->z = t;
                this->w = t;
            }


            /**
            * @brief Constrói a partir de um array
            *
            * @param a - Array para construir o vetor
            */
            vec4( const T *a ) {
                this->x = a[0];
                this->y = a[1];
                this->z = a[2];
                this->w = a[3];
            }


            /**
             * @brief Constrói um vetor explicitamente
             *
             * @param x - Compomente X
             * @param y - Componente Y
             */
            vec4( const T x, const T y, const T z, const T w ) {
                this->x = x;
                this->y = y;
                this->z = z;
                this->w = w;
            }


            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec4( vec2<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = T (0);
                this->w = T (0);
            }



            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec4( vec3<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
                this->w = T (0);
            }


            /**
             * @brief Constrói por cópia
             *
             * @param v - Vetor a ser copiado
             */
            vec4( vec4<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
                this->w = v.w;
            }


            /**
             * @brief Seta os valores do vetor explicitamente
             *
             * @param x
             * @param y
             * @param z
             */
            void set_value( const T x, const T y, const T z, const T w ) {
                this->x = x;
                this->y = y;
                this->z = z;
                this->w = w;
            }


            /**
             * @brief Retorna o tamanho desse vetor
             */
            int size() const { return 4; }

            
            /**
             * @brief Operador [] para acesso ao vetor
             *
             * @param i - Índice de acesso
             */
            T& operator [] ( int i ) {
                assert( i >= 0 && i < this->size() );
                return _array[i];
            }



            /**
             * @brief Operador soma de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec4<T>& operator + ( const vec4<T> &v ) {
                vec4<T> *vr = new vec4<T>();
                vr->x = this->x + v.x;
                vr->y = this->y + v.y;
                vr->z = this->z + v.z;
                vr->w = this->w + v.w;
                return *vr;
            }



            /**
             * @brief Operador subtração de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec4<T>& operator - ( const vec4<T> &v ) {
                vec4<T> *vr = new vec4<T>();
                vr.x = this->x - vr.x;
                vr.y = this->y - vr.y;
                vr.z = this->z - vr.z;
                vr.w = this->w - vr.w;
                return *vr;
            }
 


            /**
             * @brief Operador multiplicação das componentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec4<T>& operator * ( const vec4<T> &v ) {
                vec4<T> *vr = new vec4<T>();
                vr.x = this->x * v.x;
                vr.y = this->y * v.y;
                vr.z = this->z * v.z;
                vr.w = this->w * v.w;
                return *vr;
            }



            /**
             * @brief Operador divisão de compomentes de dois vetores
             *
             * @param v1
             * @param v2
             */
            vec4<T>& operator / ( const vec4<T> &v ) {
                vec4<T> *vr = new vec4<T>();
                vr.x = this->x / v.x;
                vr.y = this->y / v.y;
                vr.z = this->z / v.z;
                vr.w = this->w / v.w;
                return *vr;
            }



            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec4<T>& operator * ( const T s ) {
                vec4<T> *vr = new vec4<T>();
                vr.x = this->x * s;
                vr.y = this->y * s;
                vr.z = this->z * s;
                vr.w = this->w * s;
                return *vr;
            }


            /**
             * @brief Operador de escalamento
             *
             * @param v - Vetor a ser escalado
             * @param s - Fator de escala
             */
            vec4<T>& operator / ( const T s ) {
                vec4<T> *vr = new vec4<T>();
                vr->x = this->x / s;
                vr->y = this->y / s;
                vr->z = this->z / s;
                vr->w = this->w / s;
                return *vr;
            }



            /**
             * @brief Acumulador +=
             *
             * @param v
             */
            vec4<T>& operator += ( const vec4<T> &v ) {
                this->x += v.x;
                this->y += v.y;
                this->z += v.z;
                this->w += v.w;
                return *this;
            }



            /**
             * @brief Acumulador -=
             *
             * @param v
             */
            vec4<T>& operator -= ( const vec4<T> &v ) {
                this->x -= v.x;
                this->y -= v.y;
                this->z -= v.z;
                this->w -= v.w;
                return *this;
            }



            /**
             * @brief Acumulador *=
             *
             * @param v
             */
            vec4<T>& operator *= ( const vec4<T> &v ) {
                this->x *= v.x;
                this->y *= v.y;
                this->z *= v.z;
                this->w *= v.w;
                return *this;
            }


            /**
             * @brief Acumulador /=
             *
             * @param v
             */
            vec4<T>& operator /= ( const vec4<T> &v ) {
                this->x /= v.x;
                this->y /= v.y;
                this->z /= v.z;
                this->w /= v.w;
                return *this;
            }


            /**
             * @brief Operador de atribuição
             *
             * @param v
             */
            vec4<T>& operator = ( const vec4<T> &v ) {
                this->x = v.x;
                this->y = v.y;
                this->z = v.z;
                this->w = v.w;
                return *this;
            }


            /**
             * @brief Comparador de igualdade
             *
             * @param v
             */
            friend bool operator == ( const vec4<T> &v1, const vec4<T> &v2 ) {
                return ( v1.x == v2.x && v1.y == v2.y && v1.z == v2.z && v1.w == v2.w ) ? true : false;
            }

            /**
             * @brief Comparador de desigualdade
             *
             * @param v
             */
            friend bool operator != ( const vec4<T> &v1, const vec4<T> &v2 ) {
                return ( v1.x != v2.x || v1.y != v2.y || v1.z != v2.z || v1.w != v2.w ) ? true : false;
            }

            /**
             * @brief Operador para aplicar uma matriz em um vetor
             *
             * @param v - Vetor
             * @param m - Matriz
             */
            friend vec4<T>& operator * ( const matrix4<T> &m, const vec4<T> &v ) {
                vec4<T> *vr = new vec4<T>();
                vr->x = v.x * m._00 + v.y * m._01 + v.z * m._02 + v.w * m._03;
                vr->y = v.x * m._10 + v.y * m._11 + v.z * m._12 + v.w * m._13;
                vr->z = v.x * m._20 + v.y * m._21 + v.z * m._22 + v.w * m._23;
                vr->w = v.x * m._30 + v.y * m._31 + v.z * m._32 + v.w * m._33;
                return *vr;
            }


            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const vec4<T> &v ) {
                char *pOut = new char[256];
                sprintf( pOut, "(%7.4f, %7.4f, %7.4f, %7.4f)", v.x, v.y, v.z, v.w );
                out << pOut;
                delete [] pOut;
                return out;
            }
    };


    /**
     * @brief Produto escalar entre dois vetores quaisquer
     *
     * @param v1
     * @param v2
     */
    template <class T>
    inline typename T::value_type dot( const T &v1, const T &v2 ) {
        typename T::value_type r = 0;
        for(int i = 0; i < v1.size(); i++) r += v1._array[i] * v2._array[i];
        return r;
    }


    /**
     * @brief Calcula o comprimento de um vetor
     *
     * @param v
     */
    template <class T>
    inline typename T::value_type length( const T &v ) {
        typename T::value_type r = 0;
        for(int i = 0; i < v.size(); i++) r += v._array[i]*v._array[i];
        return typename T::value_type(sqrt(r));
    }

    /**
     * @brief Normaliza um vetor
     *
     * @param v
     */
    template <class T>
    inline T normalize( const T &v ) {
        typename T::value_type sum(0);
        T r;

        for(int i = 0; i < v.size(); i++)
            sum += v._array[i] * v._array[i];

        sum = typename T::value_type(sqrt(sum));
        if (sum > 0)
            for(int i = 0; i < v.size(); i++)
                r._array[i] = v._array[i] / sum;

        return r;
    }

    // return the squared norm
    template <class T>
    inline typename T::value_type square_norm( const T &v ) {
        typename T::value_type r = 0;
        for( int i = 0; i < v.size(); i++) r += v._array[i]*v._array[i];
        return r;
    }
    
};

#endif


